#include <kutils.h>
#include "fdc.h"
#include "display.h"
#include "memory.h"
#include "irq.h"
#include "keyboard.h"
#include "rtc.h"

s32int fdc_send_command(fdc_drive_t *fd, u8int command);
s32int fdc_get_data(fdc_drive_t *fd, u8int *data);
s32int fdc_send_data(fdc_drive_t *fd, u8int data);
s32int fdc_wait_interrupt(fdc_drive_t *fd);
void fdc_interrupt_handler(struct regs *r);
void fdc_motor_hard_off(u32int* ptr);
s32int fdc_motor_on(fdc_drive_t *fd);
s32int fdc_motor_off(fdc_drive_t *fd);
s32int fdc_seek(fdc_drive_t *fd, u32int sector);
s32int fdc_calibrate(fdc_drive_t *fd);
s32int fdc_reset(fdc_drive_t *fd);
s32int init_fd(u32int drive_num);
s32int floppy_drive_exists(u32int drive_num);

fdc_drive_t *floppy_drives[2];
s8int fd_exists[2];
volatile s32int fdc_interrupted = 0;

s32int fdc_send_command(fdc_drive_t *fd, u8int command)
{
	//timeout is 1 second
	u32int timeout = FDC_TIMEOUT;
	while(timeout)
	{
		if((inportb(fd->ports.status_main) & 0xC0) == 0x80)
		{
			outportb(fd->ports.data_fifo,command);
			return 1;
		}
		timeout--;
		wait(10);
	}
	return 0;
}

s32int fdc_get_data(fdc_drive_t *fd, u8int *data)
{
	//timeout is 1 second
	u32int timeout = FDC_TIMEOUT;
	while(timeout)
	{
		if((inportb(fd->ports.status_main) & 0xC0) == 0xC0)
		{
			*data = inportb(fd->ports.data_fifo);
			return 1;
		}
		timeout--;
		wait(10);
	}
	return 0;
}

s32int fdc_send_data(fdc_drive_t *fd, u8int data)
{
	return fdc_send_command(fd,data);
}

s32int fdc_wait_interrupt(fdc_drive_t *fd)
{
	if(!fd)
		return 0;

	u32int timeout = FDC_TIMEOUT;
	while(timeout)
	{
		if(!fdc_interrupted)
		{
			timeout--;
		}
		else
		{
			fdc_interrupted = 0;
			return 1;
		}
		wait(10);
	}
	fdc_interrupted = 0;
	return 0;
}

void fdc_interrupt_handler(struct regs *r)
{
	if(!r)
		return;
	//write_string("<fdcs32int>");
	fdc_interrupted = 1;
}

/*
 *  DMA Stuffs, I didn't want to build this s32into the kernel but... yea...
 *  My dreams were dashed.
 */


// Used by floppy_dma_init and floppy_do_track to specify direction
typedef enum {
    floppy_dir_read = 1,
    floppy_dir_write = 2
} floppy_dir;

//define floppy_dmalen 0x4800
u32int floppy_dmalen = 0x4800;
u8int *floppy_dmabuf = (u8int*)0x4000;//the lower memory location for our dma buffer

static void floppy_dma_init(floppy_dir dir) 
{
	union 
	{
		u8int b[4]; // 4 bytes
		u32int l;    // 1 s32int = 32-bit
	} a, c; // address and count

    a.l = (u32int) floppy_dmabuf;
    c.l = (u32int) floppy_dmalen - 1; // -1 because of DMA counting

    // check that address is at most 24-bits (under 16MB)
    // check that count is at most 16-bits (DMA limit)
    // check that if we add count and address we don't get a carry
    // (DMA can't deal with such a carry, this is the 64k boundary limit)
    if((a.l >> 24) || (c.l >> 16) || (((a.l&0xffff)+c.l)>>16)) 
	{
        write_string("Floppy Controller: buffer problem\n");
			halt();
    }

    u8int mode;
    switch(dir) 
	{
        // 01:0:0:01:10 = single/inc/no-auto/to-mem/chan2
        case floppy_dir_read:  mode = 0x46; break;

        // 01:0:0:10:10 = single/inc/no-auto/from-mem/chan2
        case floppy_dir_write: mode = 0x4a; break;

        default: 
				write_string("Floppy Controller: invalid direction\n");
				halt();
                 return; // not reached, pleases "mode user uninitialized"
    }

    outportb(0x0a, 0x06);   // mask chan 2

    outportb(0x0c, 0xff);   // reset flip-flop
    outportb(0x04, a.b[0]); //  - address low byte
    outportb(0x04, a.b[1]); //  - address high byte

    outportb(0x81, a.b[2]); // external page register

    outportb(0x0c, 0xff);   // reset flip-flop
    outportb(0x05, c.b[0]); //  - count low byte
    outportb(0x05, c.b[1]); //  - count high byte

    outportb(0x0b, mode);   // set mode (see above)

    outportb(0x0a, 0x02);   // unmask chan 2
}

/*
	End icky DMA stuffs.
*/

void fdc_motor_hard_off(u32int* ptr)
{
	fdc_drive_t* fd = (fdc_drive_t*)ptr;
	if(fd->motor_on)
	{
		outportb(fd->ports.digital_output, (u8int)((fd->index & 0x03) | 0x0C));
		fd->motor_on = 0;
	}
}

s32int fdc_motor_on(fdc_drive_t *fd)
{
	rtc_unreg_callback(&fdc_motor_hard_off);
	if(!fd->motor_on)
	{//turn the motor on
		outportb(fd->ports.digital_output, (u8int)((fd->index & 0x03) | (1 << (4 + fd->index)) | 0x0C));
		wait(500);//wait 500 milliseconds for the motor to spin up.
		fd->motor_on = 1;
		return 1;
	}
	return 1;
}

s32int fdc_motor_off(fdc_drive_t *fd)
{
	rtc_reg_callback(&fdc_motor_hard_off, (u32int*)fd, 4, 0);
	return 1;
}

s32int fdc_seek(fdc_drive_t *fd, u32int sector)
{
	s32int i = 0;
	u8int st0, cyl = 0;

	//convert absolute sector to CHS
	fd->current_cylinder = sector / (fd->heads * fd->sectors);
	u32int tmp = sector % (fd->heads * fd->sectors);
	fd->current_head = tmp / fd->sectors;
	fd->current_sector = tmp % fd->sectors + 1;

	for(i = 0; i < 3; i++)
	{
		//send CHS data and the seek command
		fdc_send_command(fd, FDC_SEEK);
   	fdc_send_data(fd, (u8int)(fd->current_head << 2));
   	fdc_send_data(fd, (u8int)fd->current_cylinder); 

		if(!fdc_wait_interrupt(fd))
		{
			write_string("Floppy Controller: seek stall ");
			continue;
		}

		//acknowledge thes32interrupt and get the result
		fdc_send_command(fd, FDC_SENSE_INTERRUPT);
		fdc_get_data(fd, &st0);
   	fdc_get_data(fd, &cyl);

		if(st0 & 0x20)
		{
			//completed a seek or a recalibrate
			if(st0 & 10)
			{
				write_string(" EC ");//equipment check error...
				continue;
			}
			else
			{
				if(cyl != fd->current_cylinder)
				{
					write_string(" BC ");//cylinder did not seek properly for some reason...
					continue;
				}
				else
				{
					//success!
					wait(50);//give the drive time to settle
					return 1;
				}
			}
		}
		else
		{
			write_string(" NC ");//did not complete the seek command
			continue;
		}	
	}
	//3 retries exausted.
	return 0;
}

s32int fdc_read(fdc_drive_t *fd, u8int *data, u32int sector, u32int offset, u32int length)
{
	u8int st0, st1, st2, rcy, rhe, rse, bps;

	if(offset > 512)
		return 0;

	//convert absolute sector to CHS
	u32int new_cyl = sector / (fd->heads * fd->sectors);
	u32int tmp = sector % (fd->heads * fd->sectors);
	u32int new_head = tmp / fd->sectors;
	u32int new_sector = tmp % fd->sectors + 1;

	if((new_cyl != fd->current_cylinder) | (new_head != fd->current_head))
	{//this means that the requested data is on a different track
		//write_string("(from disk) ");		
		if(!fdc_motor_on(fd))
			return 0;
	
		//program the data rate
		outportb(fd->ports.config_control, 0x00);

		//check if a disk is in the drive
		u8int disk_not_changed = (u8int)(inportb(fd->ports.digital_input) & 0x80);
		if(disk_not_changed)
		{
			//write_string("DNC");
			//this is gaurenteed to change the disk if it exists
			if(!fdc_seek(fd, 32))
			{
				write_string("Floppy Controller: failed seek!\n");
				return 0;
			}
			if(!fdc_seek(fd, 0))
			{
				write_string("Floppy Controller: failed seek!\n");
				return 0;
			}
		}
		disk_not_changed = (u8int)(inportb(fd->ports.digital_input) & 0x80);
		if(disk_not_changed)
		{	
			write_string("Floppy Controller: No disk in drive.\n");
			//there is no disk in the drive
			return 0;
		}

		//seek to the given sector
		if(!fdc_seek(fd, sector))
		{
			write_string("Floppy Controller: failed seek!\n");
			return 0;
		}

		floppy_dma_init(floppy_dir_read);

		//send the read command
		fdc_send_command(fd, FDC_READ_DATA);
		fdc_send_data(fd, (u8int)(fd->index | (fd->current_head << 2)));	//head and drive 
		fdc_send_data(fd, (u8int)fd->current_cylinder);
		fdc_send_data(fd, (u8int)fd->current_head);
		fdc_send_data(fd, /*fd->current_sector*/1);
		fdc_send_data(fd, 2);								// bytes per sector (2 = 512)
		fdc_send_data(fd, 18);						// the final sector to read
		fdc_send_data(fd, 0x1B);							//GAP3 length
		fdc_send_data(fd, 0xff);							//data length (0xFF because bytes per sector != 0)

		if(!fdc_wait_interrupt(fd))
		{
			write_string("Floppy Controller: Read Timeout!\n");
			return 0;
		}

		// read status information
   	fdc_get_data(fd, &st0);
		//write_hex(st0);
		fdc_get_data(fd, &st1);
		fdc_get_data(fd, &st2);
   	/*
   	 * These are cylinder/head/sector values, updated with some
   	 * rather bizarre logic, that I would like to understand.
   	 */
   	fdc_get_data(fd, &rcy);
   	fdc_get_data(fd, &rhe);
   	fdc_get_data(fd, &rse);
   	// bytes per sector, should be what we programmed in
   	fdc_get_data(fd, &bps);

	}
	else
	{
		//write_string("(from cache) ");
	}

	memcpy(data,floppy_dmabuf + (512 * (new_sector - 1)), (size_t)length);

	if(!fdc_motor_off(fd))
		return 0;

	return 1;
}

s32int fdc_calibrate(fdc_drive_t *fd)
{
	s32int i = 0;
	u8int st0, cyl = 0;

	if(!fdc_motor_on(fd))
		return 0;

	for(i = 0; i < 3; i++)
	{
		fdc_send_command(fd, FDC_RECALIBRATE);
		fdc_send_data(fd, (u8int)fd->index); //the first argument is which drive we are recalibrating

		if(!fdc_wait_interrupt(fd))
		{
			write_string("Floppy Controller: calibration stall\n");
			continue;
		}

		//acknowledge thes32interrupt and get the result
		fdc_send_command(fd, FDC_SENSE_INTERRUPT);
		fdc_get_data(fd, &st0);
   	fdc_get_data(fd, &cyl);

		if(st0 & 0x20)
		{
			//completed a seek or a recalibrate
			if(st0 & 10)
			{
				write_string(" EC ");//equipment check error...
				continue;
			}
			else
			{
				if(cyl)
				{
					write_string(" BC ");//cylinder did not return to 0 for some reason...
					continue;
				}
				else
				{
					//success!
					fdc_motor_off(fd);
					return 1;
				}
			}
		}
		else
		{
			write_string(" NC ");//did not complete the recalibrate command
			continue;
		}	
	}
	//3 retries exausted.
	return 0;
}

s32int fdc_reset(fdc_drive_t *fd)
{
	s32int i = 0;
	u8int st0, cyl = 0; 

	//make sure that an interrupt handler is installed
	irq_install_handler(6, fdc_interrupt_handler);

	//reset the controller by clearing and reseting bit 2 in the digital output register
	outportb(fd->ports.digital_output, 0x00);

	//program the data rate
	outportb(fd->ports.config_control, 0x00);

	outportb(fd->ports.digital_output, 0x0C);//reset finished

	if(!fdc_wait_interrupt(fd))
	{
		write_string("Floppy Controller: Drive ");
		write_number(fd->index);
		write_string(" Failed tos32interrupt the CPU.\n");
		fd_exists[fd->index] = 0;
		kfree(fd);
		return 0;
	}
	
	for(i = 0; i < 4; i++)
	{
		fdc_send_command(fd, FDC_SENSE_INTERRUPT);
		fdc_get_data(fd, &st0);
   	fdc_get_data(fd, &cyl);
	}

	//configure the drive
	fdc_send_command(fd,FDC_CONFIGURE);
	fdc_send_data(fd, 0x00);//this parameter is defined as all zeroes for some reason
	u8int config_bits = 
//                    7      6         5                 4          3   2   1   0
//the bits here are:| 0 |   EIS   |  EFIFO          |   POLL     | FIFO threshold |
                         (0 << 6) | (0 << 5)        | (1 << 4)   | 15;
//               No Implied Seeks | Enable the FIFO | No Polling | Large FIFO buffer |
	fdc_send_data(fd, config_bits);
	fdc_send_data(fd, 0);//pretrk

	//specify...
	fdc_send_command(fd,FDC_SPECIFY);
	fdc_send_data(fd, (u8int)((fd->step_rate_time << 4) | (fd->head_unload_time & 0x07)));
	fdc_send_data(fd, (u8int)((fd->head_load_time << 1) | 0));//a 1 here would put the controller s32into non-dma mode

	if(!fdc_calibrate(fd))
	{
		write_string("Floppy Controller: Failed Calibration!\n");
		return 0;
	}
	
	return 1;
}

s32int init_fd(u32int drive_num)
{
	ASSERT(drive_num <= 1);	

	floppy_drives[drive_num] = (fdc_drive_t*)kmalloc(sizeof(fdc_drive_t));
	fdc_drive_t *fd = floppy_drives[drive_num];

	fd->index = drive_num;

	//settings for a 1.44mb floppy
	fd->cylinders = 80;
	fd->heads = 2;
	fd->sectors = 18;

	//give bogus data in order to detect when something has not been setup properly
	fd->current_cylinder = 100;//bogus
	fd->current_head = 5;//bogus
	fd->current_sector = 20;//bogus

	fd->step_rate_time = 8;
	fd->head_load_time = 10;
	fd->head_unload_time = 240;
	fd->motor_delay = 300;

	fd->motor_on = 0;

	switch(drive_num)
	{
		case 0:
			fd->ports.status_main = 0x3F4;
			fd->ports.status_a = 0x3F0;
			fd->ports.status_b = 0x3F1;
			fd->ports.digital_input = 0x3F7;
			fd->ports.digital_output = 0x3F2;
			fd->ports.tape_drive = 0x3F3;
			fd->ports.data_rate_select = 0x3F4;
			fd->ports.config_control = 0x3F7;
			fd->ports.data_fifo = 0x3F5;
		break;
		case 1:

		break;
		default:
			
		break;
	}

	if(!fdc_reset(fd))
		return 0;	

	//u8int *floppytmp = (u8int*)kmalloc(512);
	//write_number(fdc_read(fd, floppytmp, 0, 0, 512));
	//kfree(floppytmp);
	return 1;
}

s32int floppy_drive_exists(u32int drive_num)
{
	//ask the CMOS chip about the floppy drives
	outportb(0x70, 0x10);
   u8int states = inportb(0x71);

	switch(drive_num)
	{
		case 0:
			if(states >> 4)
				return 1;
		break;
		case 1:
			if(states& 0xf)
				return 1;
		break;
		default:
			return 0;
		break;
	}
	return 0;
}

void init_fdc(void)
{
#ifdef FLUIDIUM_DEBUG
	write_string("Floppy Controller: Init\n");
#endif
	u32int i = 0;
	for(i = 0; i < 2; i++)
	{
		if(!floppy_drive_exists(i))
		{
			//write_string("\nDrive ");
			//write_number(i);
			//write_string(" was not detected.\n");
			fd_exists[i] = 0;
		}
		else
		{
			//write_string("\nDrive ");
			//write_number(i);
			//write_string(" exists.\n");
			fd_exists[i] = 1;
			if(!init_fd(i))
				fd_exists[i] = 0;
		}

	}
}


